#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <opencv2\nonfree\features2d.hpp>
#include <opencv2\features2d\features2d.hpp>
#include <opencv2\calib3d\calib3d.hpp>
//#include "PrincipalHeader.h"
//#include "../include/PrincipalHeader.h"

using namespace cv;
using namespace std;


void featureDetection(Mat img1, Mat img2, vector<DMatch>* matches, vector<Point2f>* imgpts1, vector<Point2f>* imgpts2) 
{
	// detecting keypoints
	int minHessian = 400;
	SurfFeatureDetector detector(minHessian);
	vector<KeyPoint> keypoints1, keypoints2;
	detector.detect(img1, keypoints1);
	detector.detect(img2, keypoints2);

	// descriptors
	SurfDescriptorExtractor extractor;
	Mat descriptors1, descriptors2;
	extractor.compute(img1, keypoints1, descriptors1);
	extractor.compute(img2, keypoints2, descriptors2);

	// matching descriptors
	BFMatcher matcher(NORM_L2);
	matcher.match(descriptors1, descriptors2, *matches);

	/* Trying to filter out the mistakes of the matcher by fixing a
	* limit to the distance*/
	vector<DMatch>::iterator it = matches->begin();
	for (; it != matches->end();)
	{
		if (it->distance >= 0.15)
		{
			it = matches->erase(it);
		}
		else
		{
			++it;
		}
	}

	// draw matches
	Mat img_matches;
	drawMatches(img1, keypoints1, img2, keypoints2, *matches, img_matches);

	imshow("Matches", img_matches);

	///

	for (unsigned int i = 0; i < matches->size(); i++)
	{
		imgpts1->push_back(keypoints1[(*matches)[i].queryIdx].pt);
		imgpts2->push_back(keypoints2[(*matches)[i].trainIdx].pt);

	}

}

void SFM(Mat img1, Mat img2) {

	vector<DMatch>* matches = new vector<DMatch>();
	vector<Point2f>* imgpts1 = new vector<Point2f>();
	vector<Point2f>* imgpts2 = new vector<Point2f>();
	featureDetection(img1, img2, matches, imgpts1, imgpts2);


	Mat F = findFundamentalMat(*imgpts1, *imgpts2, FM_RANSAC, 0.1, 0.99);
	//Mat_<double> E = K.t() * F * K;

	Matx34d P(1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0);

	SVD svd(F);
	Matx33d W(0, -1, 0,
		1, 0, 0,
		0, 0, 1);
	Mat_<double> R = svd.u * Mat(W) * svd.vt;
	Mat_<double> T = svd.u.col(2);

	Matx34d P1(R(0, 0), R(0, 1), R(0, 2), T(0),
		R(1, 0), R(1, 1), R(1, 2), T(1),
		R(2, 0), R(2, 1), R(2, 2), T(2));



	waitKey(0);
}

int main(int argc, char** argv)
{
	Mat img1, img2;
	img1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	img2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);

	if (argc < 2 || (!img1.data && !img2.data))
	{
		printf("No image data \n");
		return -1;
	}

	vector<DMatch>* matches = new vector<DMatch>();
	vector<Point2f>* imgpts1 = new vector<Point2f>();
	vector<Point2f>* imgpts2 = new vector<Point2f>();
	featureDetection(img1, img2, matches, imgpts1, imgpts2);


	waitKey(0);

	return 0;
}